# 导入模块
import threading
import time
import serial
import serial.tools.list_ports
import math
import matplotlib.pyplot as plt
import numpy as np
import matplotlib
# import constants as cst
# 自定义变量
port = "COM5"  # 端口号，根据自己实际情况输入，可以在设备管理器查看
bps = 115200     # 串口波特率，根据自己实际情况输入
timeout = 5       # 超时时间,None：永远等待操作，0为立即print("times:",times)返回请求结果，其他值为等待超时时间(单位为秒）
rxdata = ''    # 接收的数据
rx_data=[]
rx_buf=[]
rx_buf_show=[]
rx_buf_fin=[]
M_PI = 3.1415926
Angle_Fin = []
Distance_Fin = []
times=0
# 扫描端口
def check_uart_port():
    port_list = list(serial.tools.list_ports.comports())
    # print(port_list)
    if len(port_list) == 0:
        print('can not fine uart port')
        return False
    else:
        for i in range(0,len(port_list)):
            print(port_list[i])
    return True

# 打开串口
def open_uart(port, bps, timeout):
    try:
        # 打开串口，并返回串口对象
        uart = serial.Serial(port, bps, timeout=timeout)
        return uart
    except Exception as result:
        print("can not open uart")
        print(result)
        return False

# 发送数据
def uart_send_data(uart, txbuf):
    len = uart.write(txbuf.encode('utf-8'))  # 写数据
    return len

# 接收数据
def uart_receive_data(uart):
    if uart.in_waiting:
        # rxdata = uart.read(uart.in_waiting).decode("utf-8")   # 以字符串接收
        rxdata = uart.read().hex()  # 以16进制(hex)接收
        # print("rxdata:",type(rxdata),rxdata)  # 打印数据
        # rx_data.append(rxdata)
        # print("rxdata:",rxdata)
        lidarx4_handle(rxdata)


def lidarx4_handle(rx):
    global times
    rx_buf_show.append(rx)
    if rx=='aa' and len(rx_buf)<1:
        rx_buf.append(rx)
    elif len(rx_buf)>0 and rx_buf[0]=='aa' and rx=='55':
        rx_buf.append(rx)
        if rx_buf[-2]=='aa':
            if len(rx_buf)>2:
                rx_buf_fin=rx_buf[:-2]
                times+=1
                # print("times:",times)
                # print("rx_buf:",rx_buf)
                # print("rx_buf_fin_len:", len(rx_buf_fin))
                # print("rx_buf_fin:", rx_buf_fin)
                # change_c(rx_buf_fin)#转化为c语言的数组格式
                try:
                    Lidarx4_data_process(rx_buf_fin)
                except:
                    pass
                # print("rx_buf_show:",rx_buf_show)
                rx_buf_show.clear()
                rx_buf.clear()
                rx_buf.append('aa')
                rx_buf.append('55')
    elif len(rx_buf)>1 and rx_buf[0]=='aa'  and rx_buf[1]=='55':
        rx_buf.append(rx)

    # print("rx_buf_fin:", rx_buf_fin)
def Lidarx4_data_process(Buffer):
    global Angle_Fin
    global Distance_Fin
    # print("data:",Buffer)
    if Buffer[2]=='00':
        check_code = 0x55AA

        CT = int(Buffer[2],16) & 0x01
        LSN = int(Buffer[3],16)
        # print("LSC:", LSN)
        FSA = (int(Buffer[5],16) << 8 | int(Buffer[4],16))
        check_code ^= FSA

        LSA = (int(Buffer[7],16) << 8 | int(Buffer[6],16))
        CS = (int(Buffer[9],16) << 8 | int(Buffer[8],16))

        Distance = [0 for x in range(0, LSN)]
        Itensity = [0 for x in range(0, LSN)]
        if LSN==len(Buffer[10:])//3:
            for i in range(0,LSN*3,3):
                check_code ^= int(Buffer[10 + i],16)
                # print("i:",i,"Buffer:",int(Buffer[10 + i + 2],16),int(Buffer[10 + i + 1],16))
                data =(int(Buffer[10 + i + 2],16) << 8 | int(Buffer[10 + i + 1],16))
                check_code ^= data

                Itensity[i // 3] =((int(Buffer[10 + i + 1],16) & 0x03) << 8 | int(Buffer[10 + i],16))
                Distance[i // 3] = data >> 2
                # Distance_Fin.append(Distance[i // 3])

            check_code ^= (LSN << 8 | CT)
            check_code ^= LSA

            Angle = [0 for x in range(0, LSN)]

            if (check_code == CS):
                Angle_FSA = (FSA >> 1) // 64
                Angle_LSA = (LSA >> 1) // 64
                Angle_Diff = (Angle_LSA - Angle_FSA)
                if (Angle_Diff < 0):
                    Angle_Diff = Angle_Diff + 360
                for i in range(0,LSN):
                    if (LSN > 1):
                        Angle[i] = i * Angle_Diff / (LSN - 1) + Angle_FSA
                    else:
                        Angle[i] = Angle_FSA

                    if (Distance[i] > 0):
                        AngCorrect =math.atan(21.8 * (155.3 - Distance[i]) / (155.3 * Distance[i]))
                        Angle[i] = Angle[i] + AngCorrect * 180 / M_PI

                    Angle[i]=math.radians(Angle[i]-90)
                    # Angle_Fin.append(round(Angle[i],6))
                # print(math.degrees(Angle), Distance)
                plt_show(Angle, Distance)

                # print("Distance_len:",len(Distance),"Angle_len:",len(Angle))

                # print(Distance_Fin,Angle_Fin)

            else:
                print("error")




def plt_show(theta, r):
    plt.scatter(theta, r, s=1.0)
    plt.pause(0.01)

def change_c(data):
    for i in range(len(data)):
        print("Buffer["+str(i)+"]=0X"+data[i]+";")
# 关闭串口
def close_uart(uart):
    uart.close()


def plt_init():
    plt.ion()  # 开启interactive mode 成功的关键函数

    fig = plt.figure(figsize=(5, 5))
    ax = plt.gca(projection='polar')
    ax.set_thetagrids(np.arange(0.0, 360.0, 15.0))
    ax.set_thetamin(0.0)  # 设置极坐标图开始角度为0°
    ax.set_thetamax(360.0)  # 设置极坐标结束角度为180°
    ax.set_rlabel_position(0.0)  # 标签显示在0°
    ax.grid(True, linestyle="-", color="k", linewidth=0.5, alpha=0.5)
    ax.set_axisbelow('True')  # 使散点覆盖在坐标系之上
# 创建一个线程用来等待串口接收数据
class myThread (threading.Thread):   # 继承父类threading.Thread
    def __init__(self, uart):
        threading.Thread.__init__(self)
        self.uart = uart
    def run(self):                   # 把要执行的代码写到run函数里面 线程在创建后会直接运行run函数
        while True:
            # print("thread_uart_receive")
            uart_receive_data(self.uart)  # 接收数据
            # time.sleep(0.01)

'''
# 主函数
def main():
    # 扫描端口
    result = check_uart_port()
    if(result == False):
        return

    # 打开串口
    result = open_uart(port, bps, timeout)
    if (result == False):
        return
    else:
        uart1 = result

    # 创建一个线程用来接收串口数据
    thread_uart = myThread(uart1)
    thread_uart.start()

    while True:
        # 定时发送数据
        txbuf = "hello world"
        len = uart_send_data(uart1, txbuf)
        print("send len: ", len)
        time.sleep(1)
'''
def main():
    # 扫描端口
    result = check_uart_port()
    if(result == False):
        return

    # 打开串口
    result = open_uart(port, bps, timeout)
    if (result == False):
        return
    else:
        uart1 = result
    a=0
    while True:
        #串口接收数据方法1
        # data = uart1.read(uart1.inWaiting()).hex()
        # data = str(data)
        # if data != '':
        #     print("data:",data)

        # 串口接收数据方法2
        # rcv = uart1.read_all().hex()
        # if (len(rcv) > 1):
        #     print("data:",rcv)

        # 串口接收数据方法3
        uart_receive_data(uart1)  # 接收数据
        # # print("aaaaaaaaaaaaaa")
        # a+=1
        # if a >1050000:
        #     break
        # if
    # for i in range(len(Angle_Fin)):
    #     Angle_Fin[i]=math.radians(Angle_Fin[i])
    # plt_show(Angle_Fin,Distance_Fin)

matplotlib.use("TkAgg")
# 启动主函数
plt_init()
main()
# print(math.radians(30))
# plt_show([0.523,0.523],[5,10])
# DATA=['aa', '55', '00', '01', '53', 'ae', '53', 'ae', 'ab', '54', '00', '00', '00']#, 'cc', 'e5', '6f', 'fd', 'eb', '01', '00', '00', '00', 'fd', 'f3', '01', '00', '00', '00', 'fd', '03', '02', '00', '00', '00', 'fd', '1b', '02', '00', '00', '00', 'fd', '2f', '02', '00', '00', '00', 'fd', '43', '02', '00', '00', '00', 'fd', '5b', '02', '00', '00', '00', '60', '73', '02', '00', '00', '00', '02', '86', '02', '00', '00', '00', 'fe', '67', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', 'fe', '8b', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00']
# DATA=['aa', '55', '00', '28', 'e5', '6f', 'bd', '79', '37', '6a', 'a0', '0f', '00', '00', '00', '00', 'fd', 'eb', '01', '00', '00', '00', 'fd', 'f3', '01', '00', '00', '00', 'fd', '03', '02', '00', '00', '00', 'fd', '1b', '02', '00', '00', '00', 'fd', '2f', '02', '00', '00', '00', 'fd', '43', '02', '00', '00', '00', 'fd', '5b', '02', '00', '00', '00', '60', '73', '02', '00', '00', '00', '02', '86', '02', '00', '00', '00', 'fe', '67', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', 'fe', '8b', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00']
# Lidarx4_data_process(DATA)
